pub struct X {
    pub x: f64,
    pub y: f64,
    pub yaw: f64,
}
impl X {
    /// input u=[v,yaw_rate]
    pub fn motion_model(&self, dt: f64, v: f64, yaw_rate: f64) -> Vec<f64> {
        let x = self.x + dt * v * self.yaw.cos();
        let y = self.y + dt * v * self.yaw.sin();
        let yaw = self.yaw + dt * yaw_rate;
        vec![x, y, yaw]
    }
}
#[test]
fn test_erf() {
    ///x,y,yaw
    let mut x = vec![0., 0., 0.];
    /// dis to car ; angle to car move ; index
    let landmark0 = vec![10.08809767, -0.18236624, 0.];
    let landmark1 = vec![17.92197193, 0.54430427, 1.];
    let landmark2 = vec![15.16609828, 1.39097575, 2.];
    let landmarks = vec![landmark0, landmark1, landmark2];
    /// velocity ,yaw_rate
    let input = vec![1.27127227, 0.0213036];
    let dt = 0.1;
}
